#include <stdio.h>
#include <math.h>
#include <gd32_include.h>
#include <printf_over_x.h>
#include "arm_math.h"

/* ----------------------------------------------------------------------
 * Defines each of the tests performed
 * ------------------------------------------------------------------- */
#define MAX_BLOCKSIZE 32
#define DELTA (0.0001f)

/* ----------------------------------------------------------------------
 * Test input data for Floating point sin_cos example for 32-blockSize
 * Generated by the MATLAB randn() function
 * ------------------------------------------------------------------- */

const float32_t testInput_f32[MAX_BLOCKSIZE] =
    {
        -1.244916875853235400, -4.793533929171324800, 0.360705030233248850, 0.827929644170887320, -3.299532218312426900, 3.427441903227623800, 3.422401784294607700, -0.108308165334010680,
        0.941943896490312180, 0.502609575000365850, -0.537345278736373500, 2.088817392965764500, -1.693168684143455700, 6.283185307179590700, -0.392545884746175080, 0.327893095115825040,
        3.070147440456292300, 0.170611405884662230, -0.275275082396073010, -2.395492805446796300, 0.847311163536506600, -3.845517018083148800, 2.055818378415868300, 4.672594161978930800,
        -1.990923030266425800, 2.469305197656249500, 3.609002606064021000, -4.586736582331667500, -4.147080139136136300, 1.643756718868359500, -1.150866392366494800, 1.985805026477433800

};

const float32_t testRefOutput_f32 = 1.000000000;

/* ----------------------------------------------------------------------
 * Declare Global variables
 * ------------------------------------------------------------------- */
uint32_t blockSize = 32;
float32_t testOutput;
float32_t cosOutput;
float32_t sinOutput;
float32_t cosSquareOutput;
float32_t sinSquareOutput;

/* ----------------------------------------------------------------------
 * Max magnitude FFT Bin test
 * ------------------------------------------------------------------- */

arm_status status;

void systick_config(void);
void delay_1ms(uint32_t count);

int main(void)
{
    systick_config();
    //configure printf() output via e.g. USART
    //see function for details
    init_printf_transport();

    delay_1ms(500);
    printf("ARM cos and sin example start!\n");
    while (1)
    {
        float32_t diff;
        uint32_t i;

        for (i = 0; i < blockSize; i++)
        {
            //for each input a, compute cos(a) and sin(a)
            cosOutput = arm_cos_f32(testInput_f32[i]);
            sinOutput = arm_sin_f32(testInput_f32[i]);
            //square it (cos²(a), sin²(a))
            arm_mult_f32(&cosOutput, &cosOutput, &cosSquareOutput, 1);
            arm_mult_f32(&sinOutput, &sinOutput, &sinSquareOutput, 1);
            //add them, testOutput = cos²(a) + sin²(a)
            arm_add_f32(&cosSquareOutput, &sinSquareOutput, &testOutput, 1);
            //absolute value of difference between ref (1.0000) and test, *should* be close to 0.
            //per Pythagorean trigonometric identity, for any value a, cos²(a) + sin²(a) = 1. 
            diff = fabsf(testRefOutput_f32 - testOutput);
            printf("Diff from reference output was: %f for input %f, cos = %f, sin = %f\n", diff, testInput_f32[i], cosOutput, sinOutput);

            /* Comparison of sin_cos value with reference */
            status = (diff > DELTA) ? ARM_MATH_TEST_FAILURE : ARM_MATH_SUCCESS;

            if (status == ARM_MATH_TEST_FAILURE)
            {
                break;
            }
        }

        if (status != ARM_MATH_SUCCESS)
        {
            printf("ARM cos() and sin() tests -> FAILURE\n");
        }
        else
        {
            printf("ARM cos() and sin() tests -> SUCCESS\n");
        }
        //slightly delay
        delay_1ms(1000);
    }
}

volatile static uint32_t delay;

void systick_config(void)
{
    /* setup systick timer for 1000Hz interrupts */
    if (SysTick_Config(SystemCoreClock / 1000U))
    {
        /* capture error */
        while (1)
        {
        }
    }
    /* configure the systick handler priority */
    NVIC_SetPriority(SysTick_IRQn, 0x00U);
}

void delay_1ms(uint32_t count)
{
    delay = count;

    while (0U != delay)
    {
    }
}

void delay_decrement(void)
{
    if (0U != delay)
    {
        delay--;
    }
}

void NMI_Handler(void) {}

void HardFault_Handler(void)
{
    while (1)
        ;
}

void MemManage_Handler(void)
{
    while (1)
        ;
}

void BusFault_Handler(void)
{
    while (1)
        ;
}

void UsageFault_Handler(void)
{
    while (1)
        ;
}

void SVC_Handler(void)
{
}

void DebugMon_Handler(void)
{
}

void PendSV_Handler(void)
{
}

void SysTick_Handler(void)
{
    delay_decrement();
}
